Kalman Filter Correction with Rational Non-linear Functions: Application to Visual-SLAM

This article deals with the divergence of the Kalman filter when used on rational non-linear observation functions in the Visual SLAM framework. The context objective is to localize a vehicle and simultaneously to build a map according to environment perceived by a camera. There are many ways to fuse all data from sensors and the usual one is the Kalman filter. A main problem of this approach is the divergence due to an improper linearization of the observation model. It leads to wrong estimation which disturbs all the process. The presented method allows, under weak constraint on the observation function, to reduce the divergence effect without modifying the observation noise. In the Visual SLAM context, this method drastically improves results and gives more stability to monocular system in order to initialize landmarks.